#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

typedef struct {
    float Kp;
    float Ki;
    float Kd;
    float out_val;
    float cur_val;
    float old_error;
    float prev_prev_error;
    float cur_error;
    float integral;
    float actual_val;
} pid_struct;

float pid_figure_result_add(pid_struct* pid, float target) // 增量PID
{
    static int counter = 0;
    pid->cur_error = target - pid->cur_val;
    float derivative = pid->cur_error - pid->old_error;
    pid->integral += pid->cur_error; // 累加误差  
    pid->out_val = pid->Kp * derivative + pid->Ki * pid->cur_error + pid->Kd * (pid->cur_error - 2 * pid->old_error + pid->prev_prev_error);
    if (!counter) {
        pid->prev_prev_error = pid->cur_error;
    } else if ((counter++) == 2) counter = 0;
    pid->actual_val = pid->out_val;
    return pid->out_val;
}

float pid_figure_result_pos(pid_struct* pid, float target) // 位置PID
{
    pid->cur_error = target - pid->cur_val;
    pid->integral += pid->cur_error;
    pid->out_val = (pid->Kp * pid->cur_error) + (pid->Ki * pid->integral) + (pid->Kd * (pid->cur_error - pid->old_error));
    pid->old_error = pid->cur_error;
    pid->actual_val = pid->cur_val + pid->out_val;
    return pid->out_val;
}

void pid_init(pid_struct* pid)
{
    pid->Kp = 1.5;
    pid->Ki = 0.0;
    pid->Kd = 0.01;
    pid->integral = 0.0;
    pid->out_val  = 0.0;
    pid->old_error = 0.0;
    pid->cur_error = 0.0;
    pid->cur_val   = 0.0;
    pid->actual_val = 0.0;
}

int main()
{
    time_t rawtime;
    struct tm * timeinfo;  
    pid_struct pid = {0};
    pid_init(&pid);
    time(&rawtime);
    srand((unsigned)time(NULL));
    timeinfo = localtime(&rawtime);
    pid.cur_val = 1000;
    while (1) {
        pid_figure_result_add(&pid, 200) ;// (float)(rand()%100 + 50);
        printf("current: %0.2f out: %0.2f actual: %0.2f\n",  pid.cur_val, pid.out_val, pid.actual_val);
        pid.cur_val += pid.out_val;// + (float)(rand()%20); 根据pid算法算出来的是补偿值应该相加
        usleep(200 * 1000);
    }
    return 0;
}